#include "DrawAxisMarker.hpp"
#include <GL/glew.h>
#include <cmath>

DrawAxisMarker::DrawAxisMarker(
		double fov, double extent, double offset):
	fov(fov), markerExtent(extent), markerOffset(offset)
	{}

void DrawAxisMarker::render(int windowWidth, int windowHeight, const Eigen::Quaterniond& rot) const {
	const double halfW = double(windowWidth) * 0.5;
	const double halfH = double(windowHeight) * 0.5;
	const double halfFov = fov*0.5;

	// given a horizontal field of view of fov over the whole width of the
	// window, the unit-radius marker ball is set at a distance that will make
	// its on-screeen radius be exactly markerExtent pixels
	const double dist = halfW / (tan(halfFov)*markerExtent);
	assert(dist >= 1.0);

	// the near and far clipping planes are then set to just touch the
	// unit-radius marker ball
	const double near = dist-1.0;
	const double far = dist+1.0;

	// extent is half the width of the frustum at the near clipping plane
	// the full frustum is defined by extent (half-width and half-height at the
	// near clipping plane), plus the 'near' and 'far' values (which give both
	// the depth of the frustum, and determine how 'pointy' it is due to the
	// perspective effect of division by z
	// the extent is calculated so that the unit-radius marker ball just
	// touches the walls of the view frustum
	const double invDist2 = 1.0 / (dist*dist);
	const double extent = near / (dist * std::sqrt(1.0 - invDist2));

	// reset the projection matrix
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	// scale and translate such that the marker will be displayed
	// with radius MARKER_EXTENT (in pixels), offset by MARKER_OFFSET
	// in each direction from the lower left corner of the viewport

	const double LINE_WIDTH = 1.5;
	const double LINE_OFFSET = LINE_WIDTH/2.0 - 0.5;

	// scale into OpenGL normalised device coordinates (which are always -1,-1 to 1,1)
	glScaled(1.0/halfW, 1.0/halfH, 1.0);
	// scale and offset to pixel coordinates, with 0,0 at the centre of the window
	const double offset = markerOffset + markerExtent;
	glTranslated(-halfW + offset + LINE_OFFSET, -halfH + offset + LINE_OFFSET, 0.0);
	glScaled(markerExtent, markerExtent, 1.0);

	// apply perspective projection according to the frustum calculated above
	glFrustum(-extent, extent, -extent, extent, near, far);

	// OpenGL expects the view to be along negative Z
	// (whereas we have it along positive Z)
	glScaled(1.0, 1.0, -1.0);

	// set the marker ball 'dist' units along the z axis
	glTranslated(0.0, 0.0, dist);

	// rotate the axis markers to match the orientation of the axes in the
	// actual data visualisations
	Eigen::Projective3d tr(rot);
	glMultMatrixd(tr.data());

	// reset the modelview matrix
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	// draw the axis marker
	// (red,green,blue => x,y,z axes)
	glLineWidth(LINE_WIDTH);
	glBegin(GL_LINES);
	glColor4f(1.0f, 0.1f, 0.1f, 1.0f);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d(1.0, 0.0, 0.0);
	glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d(0.0, 1.0, 0.0);
	glColor4f(0.2f, 0.4f, 1.0f, 1.0f);
	glVertex3d(0.0, 0.0, 0.0); glVertex3d(0.0, 0.0, 1.0);
	glEnd();
	glLineWidth(1.5);
}
